#include <PID_v1.h>
#include <Metro.h>

const byte PWMPin = 3; //Analog
const byte HoldPin = 6; //Analog
const int HoldMax = 300; //Max EinstellWert
const int HoldMin = 30; //Min EinstellWert
const int SensMax = 2000; //Max Sensorwert
const int SensMin = 0; //Min Sensorwert
const int SensSamples = 20; //Anzahl Messungen (Sensor smooth)

//PID
double Setpoint, Input, Output;
PID myPID(&Input, &Output, &Setpoint,1,1,1, REVERSE);

//SensorSmooth
int SensValArray[SensSamples];

int aktPWMVal=0; //aktueller PWMwert

Metro timerUpdateSense(500);
Metro timerUpdateSerial(1000);
Metro timerUpdatePWMOutput(100);

void setup() {
  pinMode(13, OUTPUT);
  digitalWrite(13,HIGH);
  pinMode(PWMPin, OUTPUT);
  digitalWrite(PWMPin,LOW);

  Serial.begin(19200);
  
  setupSensor();
  
  //Input
  Input = BH1750_Read();
  for(int i=0;i<=SensSamples;i++)
    SensValArray[i]=Input;

  //SetPoint (PWMVal)   
  int SensVal = analogRead(HoldPin);
  SensVal = map(SensVal, 0, 1024, HoldMin, HoldMax);
  Setpoint = SensVal;
  
  if(Input>Setpoint) //Wenn Input > SetPoint dann Lampe aus
    aktPWMVal = 255;
  else
    digitalWrite(PWMPin,HIGH);
  Output = aktPWMVal;
  
  //turn the PID on
  myPID.SetMode(AUTOMATIC);

  digitalWrite(13,LOW);
}

float error = 0.0;
double oldHash = -1; //Hash der Ausgabewerte um doppelte Einträge zu vermeiden

void loop() {  
  if (timerUpdateSense.check()) {
    //Input (Lichtsensor)
    int SensVal = BH1750_Read(); 
    SensVal = digitalSmooth(SensVal, SensValArray);
    Input = SensVal;

    //SetPoint (PWMVal)   
    SensVal = analogRead(HoldPin);
    SensVal = map(SensVal, 0, 1024, HoldMin, HoldMax);
    if(abs(SensVal-Setpoint)>3) //kleinere Sprünge ignorieren
      Setpoint = SensVal;
    
    //Compute new Output
    myPID.Compute();
  }

  if (timerUpdateSerial.check()) {
    int sensRead=BH1750_Read();
    double Hash=Input+sensRead+Setpoint+Output+aktPWMVal;
    if (Hash!=oldHash){ //Werte haben sich verändert
      oldHash=Hash;
      Serial.print(Input,0); Serial.print(" Input ("); Serial.print(sensRead); Serial.print(")\t");
      Serial.print(Setpoint,0); Serial.print(" Setpoint\t");
      Serial.print(Output,0); Serial.print(" Output\t");
      Serial.print(aktPWMVal); Serial.println(" aktPWMVal");
      Serial.flush();
    }
  }

  //PWMWert schreiben
  if (timerUpdatePWMOutput.check()) {
    boolean regelungAktiv = (abs(aktPWMVal-Output)>1); //mindest Abweichung 1
    if (aktPWMVal>230){
      regelungAktiv=(Output<217);
      if(!regelungAktiv)
        aktPWMVal=255;
    }
    
    if (regelungAktiv){
      if(int(Output)>aktPWMVal){
        error = float(aktPWMVal)/float(Output);
        error = 1-error;
        if(error > 0.5){
          aktPWMVal+=2;
        }
        else{
          aktPWMVal+=1;
        }
        if (aktPWMVal>255)
          aktPWMVal=255;   
      }
      if(int(Output)<aktPWMVal){
        error = float(Output)/float(aktPWMVal);
        error = 1-error;
        if(error > 0.5){
          aktPWMVal-=2;
        }
        else {
          aktPWMVal-=1;
        }
        if (aktPWMVal<0)
          aktPWMVal=0;
      }
    } 
    analogWrite(PWMPin,aktPWMVal);
  }

  delay(1);
}

int digitalSmooth(int rawIn, int *sensSmoothArray){     // "int *sensSmoothArray" passes an array to the function - the asterisk indicates the array name is a pointer
  int j, k, temp, top, bottom;
  unsigned long total;
  static int i;
  static int sorted[SensSamples];
  boolean done;

  i = (i + 1) % SensSamples;    // increment counter and roll over if necc. -  % (modulo operator) rolls over variable
  sensSmoothArray[i] = rawIn;                 // input new data into the oldest slot

  for (j=0; j<SensSamples; j++){     // transfer data array into anther array for sorting and averaging
    sorted[j] = sensSmoothArray[j];
  }

  done = 0;                // flag to know when we're done sorting              
  while(done != 1){        // simple swap sort, sorts numbers from lowest to highest
    done = 1;
    for (j = 0; j < (SensSamples - 1); j++){
      if (sorted[j] > sorted[j + 1]){     // numbers are out of order - swap
        temp = sorted[j + 1];
        sorted [j+1] =  sorted[j] ;
        sorted [j] = temp;
        done = 0;
      }
    }
  }

  // throw out top and bottom 15% of samples - limit to throw out at least one from top and bottom
  bottom = max(((SensSamples * 15)  / 100), 1); 
  top = min((((SensSamples * 85) / 100) + 1  ), (SensSamples - 1));   // the + 1 is to make up for asymmetry caused by integer rounding
  k = 0;
  total = 0;
  for ( j = bottom; j< top; j++){
    total += sorted[j];  // total remaining indices
    k++; 
  }
  return total / k;    // divide by number of samples
}


